#define m1_a 9
#define m1_b 8
#define m1_e 10
#define m2_e 11
#define m2_a 7
#define m2_b 6
//Sensoren
#define ir_re   2
#define ir_mire 3
#define ir_mili A1
#define ir_li   A2
#define ir_hr   4
#define ir_hl   5

int randNum = 0;

int trapdet           = 0;
unsigned long timebuf = 0;

void setup() {
  for(int i=2;i<14;i++)
  {
    pinMode(i,OUTPUT);
    digitalWrite(i,LOW);
  }
    
    //Serial.begin(9600);
    
  pinMode(ir_re, INPUT);
  pinMode(ir_mire, INPUT);
  pinMode(ir_mili, INPUT);
  pinMode(ir_li, INPUT);
  pinMode(ir_hr, INPUT);
  pinMode(ir_hl, INPUT);
    
  randomSeed(analogRead(3));
    
  blink();
	
  /* int a=analogRead(0);
  analogWrite(m1_e,a);
  analogWrite(m2_e,a);
  forw();
  delay(5000);
  stop();
  bacw();
  delay(5000);
  stop(); */
}

void loop() {
  int a=analogRead(0);
  a=a/4; //von 1024 auf 256
  
  if(a<40){
    stop();
    digitalWrite(m1_e,LOW);
    digitalWrite(m2_e,LOW);
    
    while(a<40){
      a=analogRead(0);
      a=a/4;
     }
   }
  
  analogWrite(m1_e,a);
  analogWrite(m2_e,a); 
  
  
  trapdetect();
  forw();
  
  if(!get(ir_re)){
    stop();
    while(!get(ir_re)){ 
      left();
    }
      stop();
  }
 
  if(!get(ir_li)){    
    stop();
    while(!get(ir_li)){
      right();
    }
      stop();
  }    
   
  if(!get(ir_mili) || !get(ir_mire)){     
    randNum = random(9);
    stop();
    if(randNum>5){    
      while(!get(6)){
        right();
      }
    }
    else  { 
      while(!get(6)){
        left();
      }
    }      
    
    stop();
  }      
  
}

void trapdetect(){
  if(millis()-timebuf > 5000){
    
    if(trapdet>5){
      
      stop();
      bacw();
      delay(1000);
      
      stop();
      randNum = random(9);
      
      if(randNum>5){
        right();
        delay(500);
        while(!get(ir_li) || !get(ir_mili) || !get(ir_mire) || !get(ir_re)){
          right();
        }
      }
      else  { 
        left();
        delay(500);
        while(!get(ir_li) || !get(ir_mili) || !get(ir_mire) || !get(ir_re)){
          left();
        }
      }
      stop();
    }
   
    timebuf=millis();
    trapdet=0;
  }    
}

 void forw(){
  set(m1_a, HIGH);
  set(m1_b, LOW);
  set(m2_a, HIGH);
  set(m2_b, LOW);
}

void bacw(){
  set(m1_a, LOW);
  set(m1_b, HIGH);
  set(m2_a, LOW);
  set(m2_b, HIGH);  
}

void left()
{
  set(m1_a, LOW);
  set(m1_b, HIGH);  
  set(m2_a, HIGH);
  set(m2_b, LOW);
}

void right()
{
  set(m2_a, LOW);
  set(m2_b, HIGH);
  set(m1_a, HIGH);
  set(m1_b, LOW);
}
 
void set(int pin, boolean wert) {
  digitalWrite(pin, wert);
}
boolean get(int pin) {
  return digitalRead(pin);
}
  
void stop(){
  trapdet++;
  set(m1_a, LOW);
  set(m1_b, LOW);
  set(m2_a, LOW);
  set(m2_b, LOW);
  delay(300);
}
  
void blink(){
  pinMode(13, OUTPUT);
  blink1(HIGH);
  blink1(LOW);
  blink1(HIGH);
  blink1(LOW);
  blink1(HIGH);
  blink1(LOW);
  blink1(HIGH);
  blink1(LOW);
  blink1(HIGH);
  blink1(LOW);
 }
void blink1(boolean pp){
    set(13, pp);
    delay(400);
}
 
